/*
 * STMROS Plugin base class
 */

#pragma once

#ifndef STMROS__PLUGIN_HPP_
#define STMROS__PLUGIN_HPP_

#include <memory>
#include <string>
#include <vector>
#include <functional>

#include "rclcpp/rclcpp.hpp"
#include "stmros_msgs/msg/stm_heartbeat.hpp"
#include "stmros/stmros.hpp"

namespace stmros
{

using StmRosPtr = std::shared_ptr<StmRos>;

namespace plugin
{

/**
 * @brief STMROS Plugin base class
 */
class Plugin : public std::enable_shared_from_this<Plugin>
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Plugin)

  //! Message handler callback
  using HandlerCb = std::function<void(const std::vector<uint8_t>& payload)>;
  //! Tuple: Message ID, handler callback
  using HandlerInfo = std::tuple<uint8_t, HandlerCb>;
  //! Subscriptions vector
  using Subscriptions = std::vector<HandlerInfo>;

  explicit Plugin(StmRosPtr stmros_)
  : stmros(stmros_), node(nullptr)
  {
    // 节点将在插件初始化时由StmRos类设置
  }

  explicit Plugin(
    StmRosPtr stmros_, const std::string & subnode,
    const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : stmros(stmros_),
    node(rclcpp::Node::make_shared(subnode, "", options))
  {
    // 对于子节点，使用空名称，实际名称将在初始化时设置
  }

  virtual ~Plugin() = default;

  /**
   * @brief Initialize plugin with connection
   */
  virtual void initialize(std::shared_ptr<stmconn::StmConnInterface> conn) {}

  /**
   * @brief Return vector of STM message subscriptions (handlers)
   */
  virtual Subscriptions get_subscriptions() = 0;

  virtual rclcpp::Node::SharedPtr get_node() const
  {
    return node;
  }

  virtual rclcpp::Logger get_logger() const
  {
    return node->get_logger();
  }

  virtual rclcpp::Clock::SharedPtr get_clock() const
  {
    return node->get_clock();
  }

protected:
  StmRosPtr stmros;                 // stmros back link
  rclcpp::Node::SharedPtr node;     // most of plugins uses sub-node

  /**
   * Make subscription to message.
   */
  template<class _C>
  HandlerInfo make_handler(
    const uint8_t id, void (_C::* fn)(const std::vector<uint8_t>& payload))
  {
    auto bfn = std::bind(
      fn, std::static_pointer_cast<_C>(shared_from_this()), std::placeholders::_1);

    return HandlerInfo {id, bfn};
  }
};

//! A factory class to help initialize plugin node from constructor
class PluginFactory
{
public:
  PluginFactory() = default;
  virtual ~PluginFactory() = default;

  virtual Plugin::SharedPtr create_plugin_instance(StmRosPtr stmros, const std::string & subnode = "", const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) = 0;
};

//! Helper template to make plugin factories
template<typename _T>
class PluginFactoryTemplate : public PluginFactory
{
public:
  PluginFactoryTemplate() = default;
  virtual ~PluginFactoryTemplate() = default;

  Plugin::SharedPtr create_plugin_instance(StmRosPtr stmros, const std::string & subnode = "", const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) override
  {
    static_assert(
      std::is_base_of<Plugin, _T>::value,
      "Plugin should be derived from stmros::plugin::Plugin");
    return std::make_shared<_T>(stmros, subnode, options);
  }
};

} // namespace plugin
} // namespace stmros

#endif // STMROS__PLUGIN_HPP_